#ifndef FUNC_H_
#define FUNC_H_

#include "DefaultDefine.h"
#include "VideoInput.h"
#include "VideoOutput.h"
#include "VideoProcSubSys.h"
#include "VideoBuffer.h"
#include "Mipi.h"

#include "mpi_sys.h"
#include "sdk.h"
#include "hi_comm_vpss.h"
#include "mpi_vpss.h"
#include "hi_comm_vgs.h"
#include "osd_img.h"
#include "ive_img.h"
#include "vgs_img.h"
#include "misc_util.h"
#include "audio_test.h"
#include "hisignalling.h"
#include "Yolov2.h"
#include "LaneDetect.h"

using namespace std;

namespace hiych {

std::pair<SIZE_S,PIC_SIZE_E> getPictureSize(const VideoInput& vi)
{
    SIZE_S picSize;
    PIC_SIZE_E picCate;
    if(vi.isConfigSetComplete()==false)
        return std::make_pair(picSize,picCate);

    SAMPLE_COMM_VI_GetSizeBySensor(vi.getParam()->astViInfo[0].stSnsInfo.enSnsType,&picCate);

    SAMPLE_COMM_SYS_GetPicSize(picCate,&picSize);

    return std::make_pair(picSize,picCate);
}

bool VpssUnBindVo(const VideoProcSubSys& vpss,const VideoOutput& vo)
{
    bool ret = (HI_SUCCESS == SAMPLE_COMM_VPSS_UnBind_VO(
        vpss.getParam()->grpID,
        vpss.getParam()->channelConfig[vpss.getParam()->usedChannelNum-1].channelId,
        vo.getParam()->VoDev,
        0
    ));

    if(ret == false)
        std::cout << "\nVpssUnBindVo == false\n";

    return ret;
}

bool ViStart(const VideoInput& vi)
{
    static const uint32_t frmRateDef = 30;
    VideoInput::DATA_VI_TYPE *viCfg = vi.getParam();
    SAMPLE_SNS_TYPE_E snsType = viCfg->astViInfo[0].stSnsInfo.enSnsType;
    ISP_CTRL_PARAM_S ispCtrlParam;
    HI_U32 frmRate;
    HI_S32 ret = HI_FAILURE;

    ret = SAMPLE_COMM_VI_SetParam(viCfg);
    if (ret != 0) {
        std::cout<< "\nSAMPLE_COMM_VI_SetParam == HI_FAILURE\n";
        return ret == HI_SUCCESS;
    }

    SAMPLE_COMM_VI_GetFrameRateBySensor(snsType, &frmRate);
    ret = HI_MPI_ISP_GetCtrlParam(viCfg->astViInfo[0].stPipeInfo.aPipe[0], &ispCtrlParam);
    if (ret != 0) {
        std::cout << "\nHI_MPI_ISP_GetCtrlParam == HI_FAILURE\n";
        return ret == HI_SUCCESS;
    }

    ispCtrlParam.u32StatIntvl = frmRate / frmRateDef;
    ret = HI_MPI_ISP_SetCtrlParam(viCfg->astViInfo[0].stPipeInfo.aPipe[0], &ispCtrlParam);
    if (ret != 0) {
        std::cout << "\nHI_MPI_ISP_SetCtrlParam == HI_FAILURE\n";
        return ret == HI_SUCCESS; 
    }

    ret = SAMPLE_COMM_VI_StartVi(viCfg);
    if (ret != 0) {
        std::cout << "\nSAMPLE_COMM_VI_StartVi == HI_FAILURE\n";
        return ret == HI_SUCCESS;
    }

    return ret == HI_SUCCESS;
}

bool ViStop(const VideoInput& vi)
{
    bool ret = (HI_SUCCESS == SAMPLE_COMM_VI_StopVi(
        vi.getParam()
    ));

    if(ret == false)
        std::cout << "\nViStop == false\n";

    return ret;
}

bool VpssStart(const VideoProcSubSys& vpss)
{
    hiych::VideoProcSubSys::DATA_VPSS_TYPE* cfg = vpss.getParam();
    VPSS_GRP grpId = cfg->grpID;
    HI_S32 ret = HI_FAILURE;

    ret = HI_MPI_VPSS_CreateGrp(grpId, &cfg->grpAttr);
    if (ret != 0) {
        std::cout << "\nVpssStart HI_MPI_VPSS_CreateGrp == HI_FAILURE\n";
        return ret == HI_SUCCESS;
    }

    for (int i = 0; i < cfg->usedChannelNum; i++) {
        ret = HI_MPI_VPSS_SetChnAttr(grpId, cfg->channelConfig[i].channelId, &cfg->channelConfig[i].channelAttr);
        if (ret != 0) {
            std::cout << "\nVpssStart HI_MPI_VPSS_SetChnAttr == HI_FAILURE\n";
            return ret == HI_SUCCESS;
        }

        ret = HI_MPI_VPSS_EnableChn(grpId, cfg->channelConfig[i].channelId);
        if (ret != 0) {
            std::cout << "\nVpssStart HI_MPI_VPSS_EnableChn == HI_FAILURE\n";
            return ret == HI_SUCCESS;
        }
    }

    ret = HI_MPI_VPSS_StartGrp(grpId);
    if (ret != 0) {
        std::cout << "\nVpssStart HI_MPI_VPSS_StartGrp == HI_FAILURE\n";
        return ret == HI_SUCCESS;
    }
    return ret == HI_SUCCESS;
}

bool VpssStop(const VideoProcSubSys& vpss)
{
    VPSS_GRP grpId = vpss.getParam()->grpID;
    HI_S32 ret = HI_FAILURE;

    for (int i = 0; i < vpss.getParam()->usedChannelNum ; i++) {
        ret = HI_MPI_VPSS_DisableChn(grpId, vpss.getParam()->channelConfig[i].channelId);
        if (ret != 0) {
            std::cout << "\nVpssStop HI_MPI_VPSS_DisableChn with channeID == " \
                      <<vpss.getParam()->channelConfig[i].channelId \
                      << "== HI_FAILURE\n";
            return ret == HI_SUCCESS;
        }
    }

    ret = HI_MPI_VPSS_StopGrp(grpId);
    if (ret != 0) {
        std::cout << "\nVpssStop HI_MPI_VPSS_CreateGrp == HI_FAILURE\n";
        return ret == HI_SUCCESS;
    }

    ret = HI_MPI_VPSS_DestroyGrp(grpId);
    if (ret != 0) {
        std::cout << "\nVpssStop HI_MPI_VPSS_CreateGrp == HI_FAILURE\n";
        return ret == HI_SUCCESS;
    }

    return ret == HI_SUCCESS;
}

bool ViUnBandVpss(const VideoInput& vi,const VideoProcSubSys& vpss)
{
    bool ret = (HI_SUCCESS == SAMPLE_COMM_VI_UnBind_VPSS(
        vi.getParam()->astViInfo[0].stPipeInfo.aPipe[0],
        vi.getParam()->astViInfo[0].stChnInfo.ViChn,
        vpss.getParam()->grpID
    ));

    if(ret == false)
        std::cout << "\nViUnBandVpss == false\n";

    return ret;
}

bool VoStart(const VideoOutput& vo)
{
    if(vo.isConfigSetComplete() == false)
        return false;
    
    bool ret = (HI_SUCCESS == SampleCommVoStartMipi(vo.getParam()));

    if(ret == false)
        std::cout << "\nVoStart == false\n";

    return ret;
}

bool VoStop(const VideoOutput& vo)
{
    bool ret = (HI_SUCCESS == SAMPLE_COMM_VO_StopVO(vo.getParam()));

    if(ret == false)
        std::cout << "\nVoStop == false\n";

    return ret;
}

bool ViBindVpss(const VideoInput& vi,const VideoProcSubSys& vpss)
{
    MPP_CHN_S srcChn;
    MPP_CHN_S dstChn;

    srcChn.enModId = HI_ID_VI;
    srcChn.s32DevId = vi.getParam()->astViInfo[0].stPipeInfo.aPipe[0];
    srcChn.s32ChnId = vi.getParam()->astViInfo[0].stChnInfo.ViChn;

    dstChn.enModId = HI_ID_VPSS;
    dstChn.s32DevId = vpss.getParam()->grpID;
    dstChn.s32ChnId = vpss.getParam()->channelConfig[vpss.getParam()->usedChannelNum-1].channelId;

    int ret = HI_MPI_SYS_Bind(&srcChn, &dstChn);
    if (ret != 0) {
        std::cout<< "\nViBindVpss HI_MPI_SYS_Bind == HI_FAILURE\n";
        return ret == HI_SUCCESS;
    }

    return ret == HI_SUCCESS;
}

bool VpssBindVo(const VideoProcSubSys& vpss,const VideoOutput& vo)
{
    bool ret = (HI_SUCCESS == SAMPLE_COMM_VPSS_Bind_VO(
        vpss.getParam()->grpID,
        vpss.getParam()->channelConfig[vpss.getParam()->usedChannelNum-1].channelId,
        vo.getParam()->VoDev,
        0
    ));

    if(ret == false)
        std::cout << "\nVpssBindVo == false\n";

    return ret;
}

bool VI_VPSS_VO_Create(const VideoInput& vi,const VideoProcSubSys& vpss,const VideoOutput& vo)
{
    return ViStart(vi) && VpssStart(vpss) && VoStart(vo) && ViBindVpss(vi, vpss) && VpssBindVo(vpss, vo);
}

bool VI_VPSS_VO_Release(const VideoInput& vi,const VideoProcSubSys& vpss,const VideoOutput& vo)
{
    return VpssUnBindVo(vpss,vo) &&  ViUnBandVpss(vi,vpss);
}

bool VI_VPSS_VO_Stop(const VideoInput& vi,const VideoProcSubSys& vpss,const VideoOutput& vo)
{
    return VoStop(vo) && VpssStop(vpss) && ViStop(vi);
}

bool MIPI_Init(const VideoBuffer& vb)
{
    bool ret = (HI_SUCCESS == SAMPLE_COMM_SYS_Init(vb.getParam()));
    
    if(ret == false)
        std::cout << "\nMipiInit == false\n";

    return ret;
}

bool MIPI_Close(HI_S32 mipiID)
{
    bool ret = (HI_SUCCESS == SAMPLE_VO_DISABLE_MIPITx(mipiID));
    SampleCloseMipiTxFd(mipiID);
    system("echo 0 > /sys/class/gpio/gpio55/value");
    return ret;
}

std::vector<DetectObjInfo> _NNIE_Detection_PrintResult(SVP_BLOB_S *pstDstScore, SVP_BLOB_S *pstDstRoi,
    SVP_BLOB_S *pstClassRoiNum, HI_FLOAT f32PrintResultThresh)
{
    HI_U32 i = 0, j = 0;
    HI_U32 u32RoiNumBias = 0;
    HI_U32 u32ScoreBias = 0;
    HI_U32 u32BboxBias = 0;
    HI_FLOAT f32Score = 0.0f;
    HI_S32 *ps32Score = SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_S32, pstDstScore->u64VirAddr);
    HI_S32 *ps32Roi = SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_S32, pstDstRoi->u64VirAddr);
    HI_S32 *ps32ClassRoiNum = SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_S32, pstClassRoiNum->u64VirAddr);
    HI_U32 u32ClassNum = pstClassRoiNum->unShape.stWhc.u32Width;
    HI_S32 s32XMin = 0, s32YMin = 0, s32XMax = 0, s32YMax = 0;

    std::vector<DetectObjInfo> vec;
    DetectObjInfo info;

    int currentCls = 0;

    u32RoiNumBias += ps32ClassRoiNum[0];
    for (i = 1; i < u32ClassNum; i++) {
        u32ScoreBias = u32RoiNumBias;
        u32BboxBias = u32RoiNumBias * SAMPLE_SVP_NNIE_COORDI_NUM;
        /* if the confidence score greater than result threshold, the result will be printed */
        if ((HI_FLOAT)ps32Score[u32ScoreBias] / SAMPLE_SVP_NNIE_QUANT_BASE >= f32PrintResultThresh &&
            ps32ClassRoiNum[i] != 0) {
            //SAMPLE_SVP_TRACE_INFO("==== The %dth class box info====\n", i);
            currentCls = i;
            }
        for (j = 0; j < (HI_U32)ps32ClassRoiNum[i]; j++) {
            f32Score = (HI_FLOAT)ps32Score[u32ScoreBias + j] / SAMPLE_SVP_NNIE_QUANT_BASE;
            if (f32Score < f32PrintResultThresh) {
                break;
            }
            s32XMin = ps32Roi[u32BboxBias + j * SAMPLE_SVP_NNIE_COORDI_NUM];
            s32YMin = ps32Roi[u32BboxBias + j * SAMPLE_SVP_NNIE_COORDI_NUM + 1]; /* to get next element of this array */
            s32XMax = ps32Roi[u32BboxBias + j * SAMPLE_SVP_NNIE_COORDI_NUM + 2]; /* to get next element of this array */
            s32YMax = ps32Roi[u32BboxBias + j * SAMPLE_SVP_NNIE_COORDI_NUM + 3]; /* to get next element of this array */
            //SAMPLE_SVP_TRACE_INFO("%d %d %d %d %f\n", s32XMin, s32YMin, s32XMax, s32YMax, f32Score);

            info.box.xmin = s32XMin;
            info.box.xmax = s32XMax;
            info.box.ymin = s32YMin;
            info.box.ymax = s32YMax;
            info.cls = currentCls;
            info.score = f32Score;
            vec.push_back(info);
        }

        u32RoiNumBias += ps32ClassRoiNum[i];
    }
    return vec;
}

RectBox constructRect(int left_x, int left_y, int right_x, int right_y)
{
    RectBox rect;
    rect.xmin = left_x;
    rect.ymin = left_y;
    rect.xmax = right_x;
    rect.ymax = right_y;
    return rect;
}

int ROUND_UP(int x, int __ALIGN = 2)
{
    if (x % __ALIGN == 0) {
        return x;
    } else {
        switch (__ALIGN) {
        case 2:
        case 4:
        case 8:
        case 16:
        case 32:
            return (x + __ALIGN - 1) & (~(__ALIGN - 1));
        default:
            return (x + __ALIGN - 1) - (x + __ALIGN - 1) % __ALIGN;
        }
    }
}

bool RectBoxContain(RectBox big, RectBox small)
{
    return (        
        big.xmin <= small.xmin && 
        big.ymin <= small.ymin && 
        big.xmax >= small.xmax && 
        big.ymax >= small.ymax);
}

std::vector<DetectObjInfo> getDetectInfo(Yolov2& detect, IVE_IMAGE_S& img, VIDEO_FRAME_INFO_S& frm, uint32_t color)
{
    std::vector<DetectObjInfo> detectInfo = detect.getResult(img, 0.55);

    RectBox big = constructRect(640, 150 , 1280, 930);
    MppFrmDrawRects(&frm, &big, 1 , color, ROUND_UP(6));

    if(!detectInfo.empty())
    {
        for(auto &info : detectInfo)
        {
            RectBoxTran(&info.box, 640, 384, 1920, 1080);
            MppFrmDrawRects(&frm, &info.box, 1 , color, ROUND_UP(6));
        }
    }
    return detectInfo;
}

pair<RectBox, int> getLaneInfo(VIDEO_FRAME_INFO_S& frm, VIDEO_FRAME_INFO_S& resizeFrm, uint32_t color)
{
    pair<RectBox, int> tmp = LaneDetect::getResult(resizeFrm);

    VGS_DRAW_LINE_S line;

    RectBox boxs = tmp.first;
    const int screenXMid = 1920 / 2;
    const int screenYMid = 1080 / 2;

    if(boxs.xmax > boxs.xmin && boxs.ymax > boxs.ymin)
    {
        int rect_mid_x = (boxs.xmax - boxs.xmin) / 2 + boxs.xmin;
        int rect_mid_y = (boxs.ymax - boxs.ymin) / 2 + boxs.ymin;

        line.stStartPoint.s32X = ROUND_UP(rect_mid_x);
        line.stStartPoint.s32Y = ROUND_UP(rect_mid_y);

        line.stEndPoint.s32X = ROUND_UP(screenXMid);
        line.stEndPoint.s32Y = ROUND_UP(screenYMid);
        line.u32Color = color;
        line.u32Thick = 6;

        VgsDrawLines(&frm, &line, 1);

        MppFrmDrawRects(&frm, &tmp.first, 1, color, ROUND_UP(6));
    }
    return tmp;
}

bool processLaneInfo(pair<RectBox, int> laneInfo, unsigned int uartID)
{
    char leftBuffer[4] = {0, 2, 0, 1};
    char rightBuffer[4] = {0, 2, 0, 2};
    char noneOffsetBuffer[4] = {0, 2, 0, 3};

    const int OFFSET_WARNING = 250;

    RectBox box = laneInfo.first;
    int offset = laneInfo.second;
    
    if(abs(offset) > OFFSET_WARNING )
    {
        if(offset < 0)
            HisignallingMsgSend(uartID, leftBuffer, sizeof(leftBuffer)/sizeof(leftBuffer[0]));
        else
            HisignallingMsgSend(uartID, rightBuffer, sizeof(rightBuffer)/sizeof(rightBuffer[0]));
    }
    else
        HisignallingMsgSend(uartID, noneOffsetBuffer, sizeof(noneOffsetBuffer)/sizeof(noneOffsetBuffer[0]));

    if(box.xmax- box.xmin < 20 && box.ymax - box.ymin < 20)
        return false;
    
    return true;
}

bool processNumInfo(std::vector<DetectObjInfo> detectNumInfo)
{
    if(detectNumInfo.empty())
        return false;

    const int AUDIO_RIGHT_1 = 0;
    const int AUDIO_RIGHT_2 = 1;
    const int AUDIO_LEFT_3 = 2;
    const int AUDIO_LEFT_4 = 3;

    const int NUM_1 = 2;
    const int NUM_2 = 3;
    const int NUM_3 = 4;
    const int NUM_4 = 5;

    static int lastAudioNum = -1;
    static int currentAudioNum = -1;
    static int count = 0;

    static bool flag = false;
    RectBox big = constructRect(640, 150 , 1280, 930);

    for(auto &info: detectNumInfo)
    {
        RectBox tmp = info.box;
        std::thread audio([&]{
            switch(info.cls)
            {
                case NUM_1:
                {
                    if( RectBoxContain(big, tmp) )
                    {
                        cout << "--------------num is --------------" << 1 << endl;
                        flag = true;
                        currentAudioNum = AUDIO_RIGHT_1;
                    }
                    break;
                }
                case NUM_2:
                {
                    if( RectBoxContain(big, tmp) )
                    {
                        cout << "--------------num is --------------" << 2 << endl;
                        flag = true;
                        currentAudioNum = AUDIO_RIGHT_2;
                    }
                    break;
                }
                case NUM_3:
                {
                    if( RectBoxContain(big, tmp) )
                    {
                        cout << "--------------num is --------------" << 3 << endl;
                        flag = true;
                        currentAudioNum = AUDIO_LEFT_3;
                    }
                    break;                   
                }
                case NUM_4:
                {
                    if( RectBoxContain(big, tmp) )
                    {
                        cout << "--------------num is --------------" << 3 << endl;
                        flag = true;
                        currentAudioNum = AUDIO_LEFT_4;
                    }
                    break;
                }
                default:
                    break;
            }

            if(currentAudioNum != lastAudioNum)
            {
                lastAudioNum = currentAudioNum;
                AudioTest(currentAudioNum, -1);
                count = 0;
            }
            else
            {
                if(count < 1 && currentAudioNum != -1)
                    AudioTest(currentAudioNum, -1);
                count++;
            }
        });

        audio.join();
        if(flag)
            break;
    }

    flag = false;
    return true;
}


bool processKeyInfo(std::vector<DetectObjInfo> detectKeyInfo)
{
    if(detectKeyInfo.empty())
        return false;

    const int HEAD = 1;
    const int HAND = 2;
    const int KNEE = 3;
    const int FOOT = 4;

    const int AUDIO_FOOT_Front = 4;
    const int AUDIO_KNEE_Front = 5;
    const int AUDIO_HAND_Front = 6;
    const int AUDIO_HEAD_Front = 7;

    static int lastAudioNum = -1;
    static int currentAudioNum = -1;
    static OSD osd;

    RectBox big = constructRect(640, 150 , 1280, 930);
    
    for(auto &info : detectKeyInfo)
    {
        RectBox tmp =info.box;
        std::thread audio( [&] {
            switch(info.cls)
            {
                case HEAD:
                {
                    if(RectBoxContain(big, tmp))
                    {
                        cout << "\n--------------RectBoxContain HEAD--------------" << endl;
                        osd.showStr("RectBoxContain HEAD", 10, 130);
                        currentAudioNum = AUDIO_HEAD_Front;
                    }
                    break;
                }
                case HAND:
                {
                    if(RectBoxContain(big, tmp))
                    {
                        cout << "\n--------------RectBoxContain HAND--------------" << endl;
                        osd.showStr("RectBoxContain HAND", 10, 130);
                        currentAudioNum = AUDIO_HAND_Front;
                    }
                    break;
                }
                case FOOT:
                {
                    if(RectBoxContain(big, tmp))
                    {
                        cout << "\n--------------RectBoxContain FOOT--------------" << endl;
                        osd.showStr("RectBoxContain FOOT", 10, 130);
                        currentAudioNum = AUDIO_FOOT_Front;
                    }
                    break;
                }
                case KNEE:
                {
                    if(RectBoxContain(big, tmp))
                    {
                        cout << "\n--------------RectBoxContain KNEE--------------" << endl;
                        osd.showStr("RectBoxContain KNEE", 10, 130);
                        currentAudioNum = AUDIO_KNEE_Front;
                    }
                    break;
                }
                default:
                    break;
            }

            if(currentAudioNum != lastAudioNum)
            {
                lastAudioNum = currentAudioNum;
                AudioTest(currentAudioNum, -1);
            }

        });
        audio.join();
    }

    return true;
}

}

#endif